#!/usr/bin/env python
# -*- coding: UTF-8 -*-
__author__ = 'Changer'


import rospy
import random
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import String


def compare(elem):
    num = int(elem[elem.find("_")+ 1 :], 16)
    return num

class PatrolNav:
    def __init__(self):

        # 到达一个目标后需要等待的时间
        self.rest_time     = rospy.get_param("~rest_time", 2) 
        # 完成一圈后是否循环
        self.keep_patrol   = rospy.get_param("~keep_patrol",   False) 
        # 是否使用随机目标点巡逻，该参数值只有在keep_patrol参数设置为true事才生效
        self.random_patrol = rospy.get_param("~random_patrol", False) 
         # 巡逻的类型，当循环巡逻为false时，进行设置
        # 0:使用巡逻圈数patrol_loop,当巡逻指定的圈数后停止;
        # 1:使用巡逻时间patrol_time,当巡逻指定的时间后返回起始点停止;
        self.patrol_type   = rospy.get_param("~patrol_type", 0)
        # 当keep_patrol设置为false后有效，设置巡航的圈数.按照顺序将所有目标点巡逻一遍就是走了一圈。
        self.patrol_loop   = rospy.get_param("~patrol_loop", 4)
        # 配置巡逻的时间,当检测到巡逻时间到达指定时间后,就返回到起始点.设置的巡逻时间单位是分钟。
        self.patrol_time   = rospy.get_param("~patrol_time", 5)
        # 导航点
        self.locations = rospy.get_param('~locations')

        self.alive = False

        #load all navigation target pose
        self.sequence = list()
        self.waypoints = dict()

        for key, value in self.locations.items():
            self.sequence.append(key)
            self.waypoints[key] = Pose(Point(value[0][0], value[0][1], value[0][2]), Quaternion(value[1][0], value[1][1], value[1][2], value[1][3]))

        self.sequence.sort(key = compare)
        rospy.loginfo(self.sequence)
        # Goal state return values
        self.goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
                       'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST']

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self.move_base.wait_for_server(rospy.Duration(5))   ##############
        rospy.loginfo("Connected to move base server")

    def run(self):

        # Variables to keep track of success rate, running time.
        loop_cnt = 0      #循环次数
        n_goals  = 0
        n_successes  = 0  #成功的巡航点数量
        target_num   = 0  #目标点数量
        running_time = 0  #运行时间
        waypoint = ""
        start_time = rospy.Time.now()
        waypoints_cnt = len(self.waypoints)

        rospy.loginfo("Starting position navigation")

        #main loop and run through a sequence of wypoints
        while not rospy.is_shutdown():
            #if set keep_patrol is true
            if self.keep_patrol == False:
                if self.patrol_type == 0:  #use patrol loop
                    if target_num == waypoints_cnt:
                        if loop_cnt < self.patrol_loop -1:
                            target_num = 0
                            loop_cnt += 1
                            rospy.logwarn("Left patrol loop cnt: %d", self.patrol_loop-loop_cnt)
                        else:
                            rospy.logwarn("Now patrol loop over, exit..")
                            rospy.signal_shutdown('Quit')
                            break
            else:
                if self.random_patrol == False:
                    if target_num == waypoints_cnt:
                        target_num = 0
                else:
                    target_num = random.randint(0, waypoints_cnt -1)

            # 在当前点序列中取得下一个waypoint
            waypoint = self.sequence[target_num]
            
            if self.alive:
                self.send_goal(waypoint)
                rospy.loginfo("Going to: " + str(waypoint))
            else:
                continue

            # 计数自增
            target_num += 1
            n_goals += 1

            #单个航点到达限制时间
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(45))    #############
            #检查是否到达目标点
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.logerr("Timed out achieving goal")
            else:
                state = self.move_base.get_state() #从move_base中获得状态
                if state == GoalStatus.SUCCEEDED:
                    n_successes += 1
                    rospy.loginfo("GOAL SUCCEEDED~")
                else:
                    rospy.loginfo("Sorry, goal failed with err code: " + str(self.goal_states[state]))
            
            # 运行计时
            running_time = rospy.Time.now() - start_time # 当前时间-开始时间
            running_time = running_time.secs/60

            # 当完成导航后打印总共的成功/失败和已过去的时间
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +
                          str(n_goals) + " = " +
                          str(100 * n_successes/n_goals) + "%")
            rospy.loginfo("Running time: " + str(self.trunc(running_time, 1)) + " min")
            rospy.sleep(self.rest_time)

            if self.keep_patrol == False and self.patrol_time == 1:
                if running_time >= self.patrol_time:
                    rospy.logwarn("Now reach patrol_time, back to original position...")
                    self.send_goal(sequence[waypoints_cnt])
                    rospy.signal_shutdown('Quit')


    def send_goal(self, locate):
        self.goal = MoveBaseGoal()
        self.goal.target_pose.pose =  self.waypoints[locate]
        self.goal.target_pose.header.frame_id = 'map'
        self.goal.target_pose.header.stamp = rospy.Time.now()
        self.move_base.send_goal(self.goal) #将导航点传进move_base

    #将浮点数f截断/填充到n个小数位，而不进行舍入
    def trunc(self, f, n):
        slen = len('%.*f' % (n,f))
        return float(str(f)[:slen])

    def shutdown(self):
        rospy.logwarn("Stopping the patrol...")      






class Application:
    def __init__(self):
        rospy.init_node('patrol_navi_node', anonymous=True)
        rospy.Subscriber('patrol_switch', String, self.callback)
        self.nav = PatrolNav()

    def run(self):
        '''
        TODO1: shortcut key detection for ending the application
        '''
        self.nav.run()
        rospy.spin()    #never reached      

    def callback(self, data):
        if data.data == "on":
            self.nav.alive = True
        else:
            self.nav.alive = False
    


if __name__ == '__main__':
    app = Application()
    app.run()   #never reached
    

'''    
    try:
        PatrolNav()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.logwarn("patrol navigation exception finished.")
'''
